/*********************************************************************
*
*   Encoder.h - Simulation of the Quad Encoder
*
*   Copyright:
*       Software source code by Alex Morozov and Chris D. Locke is
*       licensed under a Creative Commons Attribution-Noncommercial-
*       Share Alike 3.0 United States License
*       (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)
*
*********************************************************************/


#ifndef QUAD_ENCODER_H_
#define QUAD_ENCODER_H_

#include "RobotBase.h"

#define PULSE_GAIN 10

/*********************************************************************
* This class simulates a Quad Encoder provided with the FIRST kit.
* The interface matches that of the Encoder class provided in the 
* FIRST WPI library.
*
* NOTES:
*   If the direction reverses, this class will start subtracting 
*   counts.  Need to confirm that that is how the FIRST/WPI encoder 
*   class works.
*
*********************************************************************/
class Encoder
{
private:
    double m_startMotorAccumulator; // motor accumulator value at Start() or Reset()
	float m_distancePerTick;		// distance of travel for each encoder tick
	double * m_p_motorAccumulator;  // pointer to the robot's motor accumulator
	
	void init( UINT32 aChannel, UINT32 bChannel )
	{
	    if( aChannel & 1 )
	    {
	        m_p_motorAccumulator = &RobotBase::p_robot->leftMotorAccumulator;
	    }
	    else
	    {
	        m_p_motorAccumulator = &RobotBase::p_robot->rightMotorAccumulator;
	    }
	    
	    m_distancePerTick = 1.0;
	}

public:
    /***************************************************************
    * Constructor.  Set aChannel to odd number for left wheel and 
    * even number for right wheel.
    ***************************************************************/
	Encoder( UINT32 aChannel, UINT32 bChannel, bool reverseDirection=false)
	{
	    init( aChannel, bChannel );
	}
	
	Encoder(UINT32 aSlot, UINT32 aChannel, UINT32 bSlot, UINT32 _bChannel, bool reverseDirection=false)
	{
	    init( aChannel, _bChannel );
	}
	
	// The following constructors are not supported by the simulation yet
	// Encoder(DigitalSource *aSource, DigitalSource *bSource, bool reverseDirection=false);
	// Encoder::Encoder(DigitalSource &aSource, DigitalSource &bSource, bool reverseDirection);
	//virtual ~Encoder();

    /***************************************************************
    * Start counting.  Same as Reset()
    ***************************************************************/
	void Start()
	{
	    m_startMotorAccumulator = *m_p_motorAccumulator;
	}

    /***************************************************************
    * Get pulse count since start
    ***************************************************************/
	INT32 Get()
	{
	    return( ( INT32 )( ( *m_p_motorAccumulator - m_startMotorAccumulator ) * PULSE_GAIN ) );
	}
	
    /***************************************************************
    * Reset pulse counter.  Same as Start()
    ***************************************************************/
	void Reset()
	{
	    Start();
	}
	
    /***************************************************************
    * Convert pulse count into distance traveled
    ***************************************************************/
	float GetDistance()
	{
	    return( Get() * m_distancePerTick );
	}
	
	void SetDistancePerTick(float distancePerTick)
    {
        m_distancePerTick = distancePerTick;
    }
	
	// The following functions are not supported by the simulation yet
	// double GetPeriod();
	// void SetMaxPeriod(double maxPeriod);
	// bool GetStopped();
	// bool GetDirection();
	// void SetReverseDirection(bool reverseDirection);
	// void Stop();

};

#endif

